#include <vector>
#include "QuadTree.h"
#include "Line.h"
#include <string.h>
#include <cilk/cilk.h>

using namespace std;

/*
 * Determines which quadrant the point is in (0,1,2,3) relative to the origin point
 *  1|0
 *  -+-
 *  2|3
 */
 
 inline int quadrantForPoint(Vec pt, Vec origin) {
  // Index by x, y
  static const int lut[2][2] = {{0, 3},{1,2}};
  bool xcomparison = pt.x <= origin.x;
  bool ycomparison = pt.y <= origin.y;
  return lut[xcomparison][ycomparison];
}

/*
 * Determines which quadrant the line segment is in (0,1,2,3) relative to the origin point
 * Returns -1 if the segment is in multiple quadrants
 */

int quadrantForLine(Line *line, Vec origin) {
  int q1 = quadrantForPoint(line->p1, origin);
  int q2 = quadrantForPoint(line->p2, origin);
  return (q1 == q2) ? q1 : -1;
}

QuadTree::QuadTree(Vec tl, Vec br, int max_segments, int depth) {
  this->tl = tl;
  this->br = br;
  parent = NULL;
  size = 0;
  this->depth = depth;
  this->max_segments = max_segments;
  int i;

  if (depth == 0) {
    for(i = 0; i < 4; ++i)
      children[i] = 0;
    return;
  }

  Vec midpt = (tl + br) / 2;
  children[0] = new QuadTree(midpt, br, max_segments, depth - 1);
  children[1] = new QuadTree(Vec(tl.x, midpt.y), Vec(midpt.x, br.y), max_segments, depth - 1);
  children[2] = new QuadTree(tl, midpt, max_segments, depth - 1);
  children[3] = new QuadTree(Vec(midpt.x, tl.y), Vec(br.x, midpt.y), max_segments, depth - 1);
  for(i = 0; i < 4; ++i)
    children[i]->parent = this;
}

void QuadTree::clear() {
  if (size != lines.size())
    for (int i = 0; i < 4; ++i)
      children[i]->clear();
  lines.clear();
  size = 0;
}

// Inserts list of segments into a quadtree
void QuadTree::insert(list<Line*> segments) {
  list<Line*>::iterator it; 
  size += segments.size();
  if ((depth == 0) || (size <= max_segments)) {
    for(it = segments.begin(); it != segments.end(); it++)
      lines.push_back(*it);
    return;
  }
  Vec midpt = (tl + br) / 2;
  list<Line*> child_lists[4];
  for(it = segments.begin(); it != segments.end(); ++it) {
    int quadrant = quadrantForLine(*it, midpt);
    if(quadrant == -1)
      lines.push_back(*it);
    else
      child_lists[quadrant].push_back(*it);
  }
  for(int i = 0; i < 4; ++i)
    if (!child_lists[i].empty()) children[i]->insert(child_lists[i]);
}

// int profit[5] = {0, 0, 0, 0, 0};
// int work[5] = {0, 0, 0, 0, 0};

// Tries to push as many segments further down the tree as possible
void QuadTree::push_down() {
  vector<Line*>::iterator it; 
  if ((depth == 0) || (size <= max_segments))
    return;
  Vec midpt = (tl + br) / 2;
  for(it = lines.begin(); it != lines.end(); ) {
    // work[depth]++;
    int quadrant = quadrantForLine(*it, midpt);
    if(quadrant == -1)
      it++;
    else {
      // Number of tests we save 
      // profit[depth] += size - lines.size() - children[quadrant]->size;
      children[quadrant]->lines.push_back(*it);
      it = lines.erase(it);
      children[quadrant]->size++;
    }
  }
  // printf("%d\t%d\t%d\t%d\t%d\n", profit[0], profit[1], profit[2], profit[3], profit[4]);
  // printf("%d\t%d\t%d\t%d\t%d\n", work[0], work[1], work[2], work[3], work[4]);
  for(int i = 0; i < 4; ++i)
    if (children[i]->size) children[i]->push_down();
}

// Initial update of quadtree to assign segments
void QuadTree::updateQuadTree(vector<Line*> segments) {
  size = segments.size();
  if ((depth == 0) || (size <= max_segments)) {
    this->lines = segments;
    return;
  }
  Vec midpt = (tl + br) / 2;
  vector<Line*>::iterator current_segment;
  vector<Line*> child_lists[4];
  for(current_segment = segments.begin(); current_segment != segments.end(); ++current_segment) {
    int quadrant = quadrantForLine(*current_segment, midpt);
    if(quadrant == -1)
      lines.push_back(*current_segment);
    else
      child_lists[quadrant].push_back(*current_segment);
  }
  for(int i = 0; i < 4; ++i)
    if (!child_lists[i].empty()) children[i]->updateQuadTree(child_lists[i]);
}

QuadTree::~QuadTree() {
  for(int i = 0; i < 4; i++)
    if(children[i]) delete children[i];
}
